/*****************************************************************************
** Includes
*****************************************************************************/

#include <sstream>
#include "../include/packet.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

/*****************************************************************************
** Implementation
*****************************************************************************/

PacketFinderBase::PacketFinderBase() :
    state(waitingForStx), verbose(false)
{
}


/*****************************************************************************
** Public
*****************************************************************************/

void PacketFinderBase::configure(const BufferType & putStx, const BufferType & putEtx, unsigned int sizeMsgType,
               unsigned int sizeLengthField, unsigned int sizeMsgID,
               unsigned int sizeMaxPayload, unsigned int sizeChecksumField, bool variableSizePayload)
{
  size_stx = putStx.size();
  size_etx = putEtx.size();
  size_msg_type = sizeMsgType;
  size_length_field = sizeLengthField;
  size_msgid = sizeMsgID;
  variable_size_payload = variableSizePayload;
  size_max_payload = sizeMaxPayload;
  size_payload = variable_size_payload ? 0 : sizeMaxPayload;
  size_checksum_field = sizeChecksumField;
  STX = putStx;
  ETX = putEtx;
  buffer = BufferType(size_stx + size_msg_type + size_length_field + size_msgid + size_max_payload + size_checksum_field + size_etx);
  state = waitingForStx;

  clear();
}

void PacketFinderBase::clear()
{
  state = waitingForStx;
  buffer.clear();
}

void PacketFinderBase::enableVerbose()
{
  verbose = true;
}

bool PacketFinderBase::checkSum()
{
  return true;
}

unsigned int PacketFinderBase::numberOfDataToRead()
{
  unsigned int num(0);

  switch (state)
  {
    case waitingForEtx:
      num = 1;
      break;

    case waitingForPayloadToEtx:
      num = size_payload + size_checksum_field;
      break;

    case waitingForMsgLength:
    case waitingForMsgID:
      num = 2;
      break;
    case waitingForMsgType:    
    case waitingForStx:
    case clearBuffer:
    default:
      num = 1;
      break;
  }

  if (verbose)
  {
    printf("[state(%d):%02d]", state, num);
  }
  return num;
}

void PacketFinderBase::getBuffer(BufferType & bufferRef)
{
  bufferRef = buffer;
}

void PacketFinderBase::getPayload(BufferType & bufferRef)
{
  bufferRef.clear();
  bufferRef.resize( buffer.size() - (size_stx + size_msg_type + size_length_field + size_msgid) );
  for (unsigned int i = size_stx + size_msg_type + size_length_field + size_msgid; i < buffer.size() - 1; i++) {
    bufferRef.push_back(buffer[i]);
  }
}

void PacketFinderBase::getPayload(std::string & strRef)
{
  strRef.clear();
  int rsize = buffer.size() - (size_stx + size_msg_type + size_length_field + size_msgid);
  std::stringstream ss;

  for (unsigned int i = size_stx + size_msg_type + size_length_field + size_msgid; i < buffer.size() - 1; i++) {
    ss << std::hex << std::setw(2) << std::setfill('0') << (buffer[i] & 0xff);
  }
  strRef = ss.str();
}

void PacketFinderBase::getPayloadAscii(std::string & strRef)
{
  strRef.clear();
  int rsize = buffer.size() - (size_stx + size_msg_type + size_length_field + size_msgid);
  std::stringstream ss;

  for (unsigned int i = size_stx + size_msg_type + size_length_field + size_msgid; i < buffer.size() - 1; i++) {
    ss << buffer[i];
  }
  strRef = ss.str();
}

/**
 * Checks for incoming packets.
 *
 * @param incoming
 * @param numberOfIncoming
 * @return bool : true if a valid incoming packet has been found.
 */
bool PacketFinderBase::update(const unsigned char * incoming, unsigned int numberOfIncoming)
{
  // clearBuffer = 0, waitingForStx, waitingForPayloadSize, waitingForPayloadToEtx, waitingForEtx,
  // std::cout << "update [" << numberOfIncoming << "][" << state << "]" << std::endl;
  if (!(numberOfIncoming > 0))
    return false;

  bool found_packet(false);

  if ( state == clearBuffer ) {
    buffer.clear();
    state = waitingForStx;
  }
  switch (state)
  {
    case waitingForStx:
      if (WaitForStx(incoming[0]))
      {
        if (size_length_field)
        {
          state = waitingForMsgType;
        }
        else
        {
          if (variable_size_payload)
          {
            // e.g. stargazer
            state = waitingForEtx;
          }
          else
          {
            // e.g. iroboQ
            //Todo; should put correct state
            state = waitingForPayloadToEtx;
          }
        }
      }
      break;
    case waitingForMsgType:
      if (WaitForMsgType(incoming[0]))
        state = waitingForMsgLength;
      break;
    case waitingForMsgLength:
      if (WaitForMsgLength(incoming, numberOfIncoming))
        state = waitingForMsgID;
      break;
    case waitingForMsgID:
      if (WaitForMsgID(incoming, numberOfIncoming))
        state = waitingForPayloadToEtx;
      break;      
    case waitingForEtx:
      if (waitForEtx(incoming[0], found_packet))
      {
        state = clearBuffer;
      }
      break;
    case waitingForPayloadToEtx:
      if (waitForPayloadAndEtx(incoming, numberOfIncoming, found_packet))
      {
        state = clearBuffer;
      }
      break;

    default:
      state = waitingForStx;
      break;
  }
  if ( found_packet ) {
    return checkSum();	//what happen if checksum is equal to false(== -1)?
  } else {
    return false;
  }
}

bool PacketFinderBase::WaitForStx(const unsigned char datum)
{
  bool found_stx(true);

  // add incoming datum
  buffer.push_back(datum);

  // check whether we have STX
  for (unsigned int i = 0; i < buffer.size() && i < STX.size(); i++)
  {
    if (buffer[i] != STX[i])
    {
      found_stx = false;
      buffer.pop_front();
      break;
    }
  }

  return (found_stx && buffer.size() == STX.size());
}
bool PacketFinderBase::WaitForMsgType(const unsigned char datum)
{
    buffer.push_back(datum);
    msgType = datum;
    return true;
}

bool PacketFinderBase::WaitForMsgLength(const unsigned char * incoming, unsigned int numberOfIncoming)
{
    for (unsigned int i = 0; i < numberOfIncoming; i++) {        
        buffer.push_back(incoming[i]);
    }
    size_payload = incoming[0];
    size_payload |= incoming[1] << 8;
    return true;
}

bool PacketFinderBase::WaitForMsgID(const unsigned char * incoming, unsigned int numberOfIncoming)
{
    for (unsigned int i = 0; i < numberOfIncoming; i++) {        
        buffer.push_back(incoming[i]);
    }    
    msgID = incoming[0];
    msgID |= incoming[1] << 8;
    return true;
}

bool PacketFinderBase::waitForEtx(const unsigned char incoming, bool & foundPacket)
{
  // push data
  buffer.push_back(incoming);

  // check when we need to wait for etx
  // if minimum payload size is 1
  if (buffer.size() < size_stx + size_etx + 1)
  {
    return false;
  }
  else
  {
    unsigned int number_of_match(0);
    for (unsigned int i = 0; i < ETX.size(); i++)
    {
      if (buffer[buffer.size() - ETX.size() + i] == ETX[i])
      {
        number_of_match++;
      }
    }

    if (number_of_match == ETX.size())
    {
      foundPacket = true;
      return true;
    }

    if (buffer.size() >= size_stx + size_max_payload + size_etx)
      return true;
    else
      return false;
  }
}

bool PacketFinderBase::waitForPayloadAndEtx(const unsigned char * incoming, unsigned int numberOfIncoming, bool & foundPacket)
{
  // push data
  for (unsigned int i = 0; i < numberOfIncoming; i++)
  {
    buffer.push_back(incoming[i]);
  }
  /*********************
  ** Error Handling
  **********************/
  if ( size_payload > size_max_payload ) {
    state = clearBuffer;
    std::ostringstream ostream;
    ostream << "abnormally sized payload retrieved, clearing [" << size_max_payload << "][" << size_payload << "]";

    ostream << std::setfill('0') << std::uppercase; //declare once, use everytime
    ostream << ", buffer: [" << std::setw(2) << buffer.size() << "][";
    for (unsigned int i = 0; i < buffer.size(); ++i ) {
      ostream << std::setw(2) << std::hex << static_cast<int>(buffer[i]) << " " << std::dec;
    }
    ostream << "\b]";

    sig_error.emit(ostream.str());
    return false;
  }

  if (buffer.size() < size_stx + size_msg_type + size_length_field + size_msgid + size_payload)
  {
    return false;
  }
  else
  {
    if (verbose) {
      std::cout << "Start check etx " << std::endl;
      for (unsigned int i = 0; i < numberOfIncoming; ++i ) {
        std::cout << std::hex << static_cast<int>(*(incoming+i)) << " ";
      }
      std::cout << std::dec << std::endl;
    }
    foundPacket = true;

	//计算校检码
    char check_code = 0;
    int buff_size = buffer.size();
    int check_code_index = buff_size - 1;
    for(int index = 0; index < check_code_index; index++){
      check_code += buffer[index];
    }
    check_code = ~check_code + 1;

    if ((buffer[check_code_index]) == (check_code & 0xff)) {
      foundPacket = true;
    } else {      
      foundPacket = false;
    }

    return true;
  }
}